/*
 * Graph.h
 *
 *  Created on: 14.03.2012
 *      Author: ignacio
 */

#ifndef GRAPH_H_
#define GRAPH_H_

//#include <string>
//using std::string;
#include <list>
using std::list;
#include <map>
using std::map;
#include <iostream>
using std::ostream;
using std::cout;
using std::endl;
#include <vector>
using std::vector;
#include "path.h"

namespace neurignacio{

enum VertexColor{WHITE, GRAY, BLACK}; // Breadth-First-Search
#define INT_MAX 32767;
typedef float real;

template <typename Type>
class BaseVertex
{
public:
	Type* value;
public:
	BaseVertex() : value(NULL) {}
	BaseVertex(Type* v): value(v) {}
	BaseVertex(Type& v): value(&v) {}
	BaseVertex(const BaseVertex& vertex) : value(vertex.value) {} // Copy constructor copies by REFERENCE!!!!
	// accessory functions
	Type getValue(void) const {return *(this->value);}
	void setValue(Type& v) {value=&v;}
	// operators
	bool operator==(const BaseVertex& v) const {return this->value==v.value;}
	bool operator!=(const BaseVertex& v) const {return this->value!=v.value;}
	bool operator>(const BaseVertex& v) const	{return this->value>v.value;}
	bool operator>=(const BaseVertex& v) const	{return this->value>=v.value;}
	bool operator<(const BaseVertex& v) const	{return this->value<v.value;}
	bool operator<=(const BaseVertex& v) const	{return this->value<=v.value;}
	friend ostream& operator<<(ostream& out, const BaseVertex& v) {out << "{" << *(v.value) << "}"; return out; }
};

template <typename Type>
class BFSVertex : public BaseVertex<Type>
/*
 * Class Vertex may represent a edge of 'weight' to a vertex 'value'.
 * 'value' could be then a pointer to, for example, a 3D-point
 */
{
	public:
		typedef real distance_t;

	public:
		// Breadth-First-Search
		VertexColor color;
		BFSVertex<Type>* predecessor;
		distance_t d;


	public:
		BFSVertex() : BaseVertex<Type>(), color(WHITE), predecessor(0), d(0) {}
		BFSVertex(Type* v) : BaseVertex<Type>(v),  color(WHITE), predecessor(0), d(0){cout << "Calling BFSVertex(*v) for " << v<< ":"<< *v << endl;}
		BFSVertex(Type& v) : BaseVertex<Type>(v),  color(WHITE), predecessor(0), d(0){cout << "Calling BFSVertex(&v) for " << v << endl;}
		template <typename T> friend ostream& operator<<(ostream& out, const BFSVertex<T>& v);
};

template <typename Type>
class DFSVertex : public BFSVertex<Type>
{
	public:
		DFSVertex<Type>* predecessor;
		typedef real timestamp_t;
	public:
		// Depth-First-Search
		timestamp_t f;
	public:
		DFSVertex() : BFSVertex<Type>(), predecessor(0), f(0) {}
		DFSVertex(Type* v) : BFSVertex<Type>(v),  f(0){cout << "CallingDSFVertex(*v) for " << v<< ":"<< *v << endl;}
		DFSVertex(Type& v) : BFSVertex<Type>(v),  f(0){cout << "Calling DFSVertex(&v) for " << v << endl;}
		template <typename T> friend ostream& operator<<(ostream& out, const DFSVertex<T>& v);
};

template <typename VertexType>
class Edge
/*
 * Weighted edge for vertex of type 'VertexType'
 */
{
public:
	typedef real weight_t;
public:
	VertexType* u;
	VertexType* v;
	weight_t weight;
public:
	Edge() : u(NULL), v(NULL), weight(1) {}
	Edge(VertexType& u, VertexType& v, weight_t w=1) : u(&u), v(&v), weight(w) {}
	Edge(VertexType* u, VertexType* v, weight_t w=1) : u(u), v(v), weight(w) {}
	Edge(const Edge& e) : u(e.u), v(e.v), weight(e.weight) {}
	void relax(void);
	template <typename T> friend ostream& operator<<(ostream& out, const Edge<T>& e);
};



// template <typename VertexType> typedef DFSVertex<VertexType> IntVertex;
// template <typename VertexType> typedef Edge<DFSVertex<VertexType> > IntEdge;

template <typename Type>
class AdjacencyList: public list<Edge<DFSVertex<Type> > >
/*
 * Adjacency-list representation
 * Contains a list of weighted egdes
 */
{
public:
	void push_back(Edge<DFSVertex<Type> >* edge) {list<Edge<DFSVertex<Type> >>::push_back(*edge);}
	void push_back(Edge<DFSVertex<Type> > edge) {list<Edge<DFSVertex<Type> >>::push_back(edge);}
	void push_back(DFSVertex<Type>& u, DFSVertex<Type>& v, real w=1.0f) {list<Edge<DFSVertex<Type> >>::push_back(Edge<DFSVertex<Type> >(u, v, w));}
	template <typename T> friend ostream& operator<<(ostream& out, const AdjacencyList<T>& adjList);
};

template <typename Type> void walkPath(DFSVertex<Type> * s, DFSVertex<Type>* v, Path<DFSVertex<Type> >* path);

template <typename Type>
class AdjacencyGraph : public map<DFSVertex<Type>*, AdjacencyList<Type>*>
/*
 * Adjacency Graph representation
 * Each key dictionary (this->first) represents a Class Vertex? (probably would be better a pointer to "typename Type value")??????????????????
 * The value (this->second) represents a list of weighted edges going out from the key Class Vertex
 */
{
public:
	void transpose();	// Transposition => Invert the direction of the edges
	void InitializeSingleSource(DFSVertex<Type> * s);
	void PrintPath(DFSVertex<Type> * s, DFSVertex<Type>* v);
	Path<DFSVertex<Type> > getPath(DFSVertex<Type> * s, DFSVertex<Type>* v);
	list<DFSVertex<Type>*> TopologicalSort();
	template <typename T> friend ostream& operator<<(ostream& out, const AdjacencyGraph<T>& graph);

	void BreadthFirstSearch(BFSVertex<Type>* s);
	void DepthFirstSearch();
	void StronglyConnectedComponets();	// Not implemented!!!
	list<Edge<DFSVertex<Type> >*> MST_Kruskal();
	list<Edge<DFSVertex<Type> >*> MST_Prim(DFSVertex<Type>* r);
	bool Bellman_Ford(DFSVertex<Type>* s); // return true is the Graph has no negative-weight cycles
	void DAG_Shortest_Path(DFSVertex<Type>* s);
	void Dijkstra(DFSVertex<Type>* s);
	void A_Star(DFSVertex<Type>* source, DFSVertex<Type>* destination);
};

template <typename Type>
typename DFSVertex<Type>::timestamp_t AStar_Heuristic(DFSVertex<Type>* u, DFSVertex<Type>* v);

template <typename Type>
class Hypergraph : public AdjacencyGraph<Type>
// Hypergraph is a container for a Graph, including Edges, Vertexes and values
// If I use 'vector' for the houses, I cannot find anymore the pointer to some element of it when the house grows and need to be reposition in memory :-(
{
public:
	typedef list<Type> House_t;
	typedef DFSVertex<Type> Vertex_t;
	typedef list<Vertex_t> VertexHouse_t;
	typedef Edge< DFSVertex<Type> > Edge_t;
	typedef list<Edge_t> EdgeHouse_t;
	typedef AdjacencyList<Type> List_t;
	typedef list<List_t> ListHouse_t;
	// iterators
	typedef typename House_t::iterator House_iterator;
	typedef typename VertexHouse_t::iterator VertexHouse_iterator;
	typedef typename EdgeHouse_t::iterator EdgeHouse_iterator;
	typedef typename ListHouse_t::iterator ListHouse_iterator;
	typedef typename Hypergraph<Type>::const_iterator Hypergraph_const_iterator;
public:
	House_t house;	// Contains all values
	VertexHouse_t vertexHouse;	// Contains all Vertexes
	EdgeHouse_t edgeHouse;	// Contains all Edges
	ListHouse_t listHouse;	// Contains all Adjacency Lists
	// *this Contains the graph connections
public:
	Hypergraph() : AdjacencyGraph<Type >(), house(), vertexHouse(), edgeHouse(), listHouse() {}
	void addEdge(Type& u, Type& v, real w=1.0f);
	void addVertex(Type& u);
	Vertex_t* getPointer(const Type& u) const;	// Returns a pointer to the vertex containing the value 'u'
	Vertex_t* getPointer(const Type& u, real distance) const;	// Returns a pointer to the vertex containing the value 'u' whose distance is greater than the given
	void Dijkstra(const Type& u) {AdjacencyGraph<Type>::Dijkstra(this->getPointer(u));}
	void Dijkstra(Vertex_t* s) {AdjacencyGraph<Type>::Dijkstra(s);}
	void A_Star(const Type& u, const Type& v) {AdjacencyGraph<Type>::A_Star(this->getPointer(u), this->getPointer(v));}
	void A_Star(Vertex_t* u, Vertex_t* v) {AdjacencyGraph<Type>::A_Star(u,v);}
	void PrintPath(const Type& u, const Type& v) {AdjacencyGraph<Type>::PrintPath(this->getPointer(u), this->getPointer(v));}
	void PrintPath(Vertex_t* u, Vertex_t* v) {AdjacencyGraph<Type>::PrintPath(u,v);}
	Path<DFSVertex<Type> > getPath(const Type& u, const Type& v) {return AdjacencyGraph<Type>::getPath(this->getPointer(u), this->getPointer(v));}
	template <typename T> friend ostream& operator<<(ostream& out, const Hypergraph<T>& graph);
};

} // end neurignacio

/*
 * ***********************************************
 * T E M P L A T E   I M P L E M E N T A T I O N *
 * ***********************************************
 */
namespace neurignacio{
/* ******************************** *
 * 			operator<< 				*
 * ******************************** *
 */

// BFSVertex - operator<<
template <typename Type>
ostream& operator<<(ostream& out, const BFSVertex<Type>& v)
{
	out << "[" << static_cast<BaseVertex<Type> >(v) << ", color=" << v.color << ", d=" << v.d;
	out << ", predecessor=";
	if (v.predecessor)
		out << *(v.predecessor->value);
	else
		out << "NULL";
	out << "]";
	return out;
}

// DFSVertex - operator<<
template <typename Type>
ostream& operator<<(ostream& out, const DFSVertex<Type>& v)
{
/*	out << "[" << static_cast<BaseVertex<T> >(v) << ", color=" << v.color << ", d=" << v.d << ", f=" << v.f;
	out << ", predecessor=";
	if (v.predecessor)
		out << static_cast<BaseVertex<T> >(v.predecessor->value);
	else
		out << "NULL";
	out << "]";
*/	out << "[" << *(v.value) << "]";	// Short version for debugging
	return out;
}

// Edge - operator<<
template <typename Type>
ostream& operator<<(ostream& out, const Edge<Type>& e)
{
	out << static_cast<Type>(*e.u) << "->";
	out << static_cast<Type>(*e.v);
/*	out << "$weight=" << e.weight<< "$";
*/	return out;
}

// AdjacencyList
template <typename Type>
ostream& operator<<(ostream& out, const AdjacencyList<Type>& adjList)
{
	for (typename AdjacencyList<Type>::const_iterator edge=adjList.begin(); edge!=adjList.end(); ++edge)
	{
		out << *edge << " ";
	}
	return out;
}

// AdjacencyGraph
template <typename Type>
ostream& operator<<(ostream& out, const AdjacencyGraph<Type>& graph)
{
	for (typename AdjacencyGraph<Type>::const_iterator adjList=graph.begin(); adjList!=graph.end(); ++adjList)
	{
		out << *(adjList->first);
		out << " " << *(adjList->second) << "\n";
	}
	return out;
}

// Hypergraph
template <typename Type>
ostream& operator<<(ostream& out, const Hypergraph<Type>& graph)
{
	for (typename Hypergraph<Type>::const_iterator adjList=graph.begin(); adjList!=graph.end(); ++adjList)
	{
		out << *(adjList->first);
		out << " " << *(adjList->second) << "\n";
	}
	return out;
}

} // end namespace neurignacio

/*
 * *******************************************
 * A L G O R I T H M   D E C L A R A T I O N *
 * *******************************************
 */

#include <queue>
using std::queue;
using std::priority_queue;
#include <iostream>
using std::cout;
using std::endl;
using std::ostream;
#include <algorithm>
using std::find;
#include <set>
using std::set;
#include <vector>
using std::vector;



namespace neurignacio{


template <typename Type>
void AdjacencyGraph<Type>::transpose()
{
	typedef DFSVertex<Type> IntVertex;
	// Create temp Graph		tG[u] = new AdjacencyList;
	AdjacencyGraph tG;
	for (typename AdjacencyGraph<Type>::iterator adjList=this->begin(); adjList!=this->end(); ++adjList)
		tG[adjList->first] = new AdjacencyList<Type>;

	for (typename AdjacencyGraph<Type>::iterator adjList=this->begin(); adjList!=this->end(); ++adjList)
	{
		IntVertex* u = adjList->first;
		for (typename AdjacencyList<Type>::iterator v = (*this)[u]->begin(); v!=(*this)[u]->end(); ++v)
			tG[v->v]->push_back(u, v->v);
	}

	// copy temp Graph to *this
	for (typename AdjacencyGraph<Type>::iterator adjList=this->begin(); adjList!=this->end(); ++adjList)
	{
		IntVertex* u = adjList->first;
		(*this)[u]->clear();
		for (typename AdjacencyList<Type>::iterator v=tG[u]->begin(); v!=tG[u]->end(); ++v)
			(*this)[u]->push_back(*v);
		tG[u]->clear();
	}

	// delete temp Graph
	for (typename AdjacencyGraph<Type>::iterator adjList=this->begin(); adjList!=this->end(); ++adjList)
			delete tG[adjList->first];
}

/* **********************
 * Breadth First Search *
 * **********************
 */
template <typename Type>
void AdjacencyGraph<Type>::BreadthFirstSearch(BFSVertex<Type>* s)
{
	typedef BFSVertex<Type> IntVertex;
//	AdjacencyGraph<VertexType>& G = this;
	for (typename AdjacencyGraph<Type>::iterator adjList=this->begin(); adjList!=this->end(); ++adjList)
		// 'adjList->first' is the Vertex 'ui'
		// 'adjList->second' is the adjancecy list of 'ui' => v0,...,vj
	{
		adjList->first->color=WHITE;
		adjList->first->d = INT_MAX;
		adjList->first->predecessor=NULL;
	}
	s->color=GRAY;
	s->d=0;
	s->predecessor = NULL;

//	cout << "Line - " << __LINE__<< endl;
//	cout << G << endl;

	queue<IntVertex*> Q;
	Q.push(s);
//	cout << "Q.size()=" << Q.size() << "\tline - " << __LINE__ << endl;
//	int count = 0;
	while (!Q.empty()!=0)
	{
		IntVertex* u = Q.front();
		Q.pop();
//		cout << "Vertex: " <<*u << " line - "<< __LINE__ << endl;
		for (typename AdjacencyList<Type>::iterator v = (*this)[u]->begin(); v!=(*this)[u]->end(); ++v)
		{
			if ((*v).v->color==WHITE)
			{
				(*v).v->color=GRAY;
				(*v).v->d=u->d+1;
//				cout << "\t" << **v << endl;
				(*v).v->predecessor=(u);
				Q.push( (*v).v);
			}
		}
//		cout << " queue size: " << Q.size() << " line - " << __LINE__ << endl;
//		if (count == 10)
//			break;
//		++count;
		u->color=BLACK;
	}
//	cout << "Final Count: " << count << endl;
}



/* ********************
 * Depth First Search *
 * ********************
 */
template <typename Type>
void DFS_Visit(AdjacencyGraph<Type>& G,DFSVertex<Type> *u, typename DFSVertex<Type>::timestamp_t& time)
{
	typedef DFSVertex<Type> IntVertex;
	++time;
//	cout << "\t\tVisiting " << *u << ".... at time "<< time  << endl;
	u->d=time;
	u->color=GRAY;
	for (typename AdjacencyList<Type>::iterator v = G[u]->begin(); v!=G[u]->end(); ++v)
	{
		if ((*v).v->color==WHITE)
		{
			(*v).v->predecessor=(u);
//			cout << "\t\tDFS_Visit(G, "<< (*v)->value << "," << time << ")"<< endl;
			DFS_Visit(G, (*v).v, time);
		}
	}
	u->color=BLACK;
	++time;
	u->f=time;
}

template <typename Type>
void AdjacencyGraph<Type>::DepthFirstSearch()
{
	typedef DFSVertex<Type> IntVertex;
//	AdjacencyGraph<VertexType>& G = this;
	for (typename AdjacencyGraph<Type>::iterator adjList=this->begin(); adjList!=this->end(); ++adjList)
		// 'adjList->first' is the Vertex 'ui'
		// 'adjList->second' is the adjancecy list of 'ui' => v0,...,vj
	{
		adjList->first->color=WHITE;
		adjList->first->predecessor=NULL;
	}

//	cout << "Start:" << endl;
//	cout << G;
	typename IntVertex::timestamp_t time = 0;
	for (typename AdjacencyGraph<Type>::reverse_iterator adjList=this->rbegin(); adjList!=this->rend(); ++adjList) // reverse_iterator is used only to match with the results of the book. forward iterator works as well.
		// 'adjList->first' is the Vertex 'ui'
		// 'adjList->second' is the adjancecy list of 'ui' => v0,...,vj
	{
//		cout << "Calling Vertex (" << *adjList->first << ")" << endl;
		if (adjList->first->color==WHITE)
			DFS_Visit(*this,adjList->first, time);
	}

}

template <typename Type>
class sortVertexByFinishingTime
{
	public:
	bool operator() (DFSVertex<Type>* u, DFSVertex<Type>* v)
	{
		return (u->f < v->f);
	}
};

template <typename Type>
class sortVertexByDistance
{
	public:
	bool operator() (BFSVertex<Type>* u, BFSVertex<Type>* v)
	{
		return (u->d < v->d);
	}
};

template <typename Type>
list<DFSVertex<Type>*> AdjacencyGraph<Type>::TopologicalSort()
{
//	AdjacencyGraph<VertexType>& G = this;
	typedef list<DFSVertex<Type>*> IntVertex;
	this->DepthFirstSearch();
	list<IntVertex*> listOfVertices;
	for (typename AdjacencyGraph<Type>::iterator adjList=this->begin(); adjList!=this->end(); ++adjList)
	{
		listOfVertices.push_back(adjList->first);
	}
	sortVertexByFinishingTime<Type> comparison;
	listOfVertices.sort(comparison);
	return listOfVertices;
}

template <typename Type>
void AdjacencyGraph<Type>::PrintPath(DFSVertex<Type> * s, DFSVertex<Type>* v)
{
/*	if (v==s)
		cout << *s;
	else if (v->predecessor)
	{
		//IntVertex* predecessorPtr = dynamic_cast<IntVertex* >((v->predecessor->));
		this->PrintPath(s, v->predecessor);
		cout << *v;
	}
	else
		cout << "No Path from " << *s << " to " << *v << " exists";
	cout << endl;
*/
	Path<DFSVertex<Type> > path;
	walkPath(s, v->predecessor, &path);
	cout << "PrintPath is done - LINE " << __LINE__ << endl;
	cout << path << endl;
}

// Auxiliary recursive function
template <typename Type> void walkPath(DFSVertex<Type> * s, DFSVertex<Type>* v, Path<DFSVertex<Type> >* path)
{
//	Path<BFSVertex<VertexType> >* path = 0;
	if (!path)
		return; // Error pointer to path was empty!

	if (v==s)
		path->push_back(*s);
	else if (v->predecessor)
	{
		walkPath(s, v->predecessor, path);
		path->push_back(*v);
	}
	else
		return;// Error - No Path from s to v
}

template <typename Type>
Path<DFSVertex<Type> > AdjacencyGraph<Type>::getPath(DFSVertex<Type> * s, DFSVertex<Type>* v)
{
	Path<DFSVertex<Type> > path;
	walkPath(s,v,&path);
	return path;
}

template <typename Type>
bool sortEdgeByWeight(Edge<DFSVertex<Type> > * a, Edge<DFSVertex<Type> > * b)
{
	return (a->weight < b->weight);
}


template <typename Type>
ostream& operator<<(ostream& out, const set<Type*>& S)		// I want to print set for debugging
{
	for (typename set<Type*>::const_iterator it=S.begin(); it!=S.end(); ++it)
	{
		out << **it << " ";
	}
	return out;
}

template <typename Type>
list<Edge<DFSVertex<Type> > *> AdjacencyGraph<Type>::MST_Kruskal()
{
//	AdjacencyGraph<VertexType>& G = this;
	typedef DFSVertex<Type> IntVertex;
	typedef Edge<DFSVertex<Type> > IntEdge;
	// 1. Create Empty Tree of Edges, A.
	list<IntEdge*> A;
	// 2. Create a map<Vertex*, Set<Vertex*> > for each of the vertices and initialize Set to empty
	map<IntVertex*, set<IntVertex*>* > S;
	list<IntEdge*> edgeList;
	for (typename AdjacencyGraph<Type>::iterator vertex=this->begin(); vertex!=this->end(); ++vertex)
	{
		// Initialize set of each vertex
		S[vertex->first]=NULL;
		// Create a list of the edges to sort (saves 1 loop)
		for (typename AdjacencyList<Type>::iterator edge=vertex->second->begin(); edge!=vertex->second->end(); ++edge)
			edgeList.push_back(&*edge);
	}

	// 3. Sort Edges in nondecreasing order by weight
	edgeList.sort(sortEdgeByWeight);
	// 4. Go through all the Edges E=(u,v)
	for (typename list<IntEdge*>::iterator edge=edgeList.begin(); edge!=edgeList.end(); ++edge)
	{
	// 		4.1 If v is not in the set that contains u
	// 			a) Add v to the set that contains u and point to that set in the map for u and v
		IntVertex* U=(*edge)->u;	// For clarity reason
		IntVertex* V=(*edge)->v;	// For clarity reason

		if (!S[U] && !S[V])	// Initial Case (Case 0): First time that we visit each of the verteces of the edge
		{
			S[U]=new set<IntVertex*>;
			S[U]->insert(U);
			S[U]->insert(V);
			S[V]=S[U];	// u and v points to the same set, which cointains so far u and v
		}
		else if (S[U] && !S[V])	// Case 1a: We already visited U and this is the first time we visit v
		{
			S[U]->insert(V); // We insert v to the set pointed by u
			S[V]=S[U];		// We make v to point to the same same than u
		}
		else if (!S[U] && S[V]) // Case 1b: We visit for first time u but we already visited v
		{
			S[V]->insert(U);
			S[U]=S[V];
		}
		else if (S[U] && S[V]) // Case 2: We already visited u and v
		{
			if (S[U]==S[V])	// Case 2.1: u and v are pointing to the same set
			{
				// Do nothing
				continue;
			}
			else if (S[U]!=S[V])	// Case 2.2: u and v are pointing to different sets
			{
				S[V]->insert(S[U]->begin(),S[U]->end());	// Merge set of u with set of v
				delete S[U];	// This pointer is not needed anymore, we clean the dynamic memory
				for (typename set<IntVertex*>::iterator it=S[V]->begin(); it!=S[V]->end();++it)
				{
					IntVertex* i=*it;
					S[i]=S[V];		// Each of the verteces from the set will point to the this set
				}
			}
		} // end else if S[U] && S[V]
		// Include actual edge into final tree of edges
		A.push_back(*edge);
	} // end for edge


	// Clean the dynamic memory of the final set
	delete (*S.begin()).second;
	// 5. Return A.
	return A;
}

template <typename Type>
list<Edge<DFSVertex<Type> >*> AdjacencyGraph<Type>::MST_Prim(DFSVertex<Type>* r)
{
//	AdjacencyGraph<VertexType>& G = this;
	typedef DFSVertex<Type> IntVertex;
	typedef Edge<DFSVertex<Type> > IntEdge;
	// 1. Create Empty Tree of Edges, A.
	list<IntEdge*> A;
	// 2. Create min-priority Q
	list<IntVertex*> Q;
	sortVertexByFinishingTime<Type> comparison;
	// 2. Initialize Graph
	for (typename AdjacencyGraph<Type>::iterator vertex=this->begin(); vertex!=this->end(); ++vertex)
	{
		vertex->first->f= INT_MAX;
		vertex->first->predecessor = NULL;
		Q.push_back(vertex->first);
	}
	// 3. Initialize root vertex 'r'
	r->f=0;
	//
	while (!Q.empty())
	{
		Q.sort(comparison);
		IntVertex* u=Q.front();
		Q.erase(Q.begin());
		cout << "U= " << *u << endl;
		AdjacencyList<Type>* adj=(*this)[u];
		for (typename AdjacencyList<Type>::iterator edge=adj->begin(); edge!=adj->end(); ++edge)
		{
			cout <<"\tV="<< *(edge->v) << "(weight: " << edge->weight << ") => ";
			if (find(Q.begin(), Q.end(), edge->v)!=Q.end() && edge->weight<edge->v->f)
			{
				edge->v->predecessor=u;
				edge->v->f=edge->weight;
			}
			cout << *(edge->v) << endl;
		}
	}

	return A;
}

/*
 * *********************** *
 * Bellman-Ford Algorithm  *
 * *********************** *
 */

template <typename Type>
void AdjacencyGraph<Type>::InitializeSingleSource(DFSVertex<Type> * s)
{
	for (typename AdjacencyGraph<Type>::iterator vertex=this->begin(); vertex!=this->end(); ++vertex)
	{
		vertex->first->d=INT_MAX;
		vertex->first->predecessor=NULL;
		vertex->first->f=INT_MAX; // Only necessary for A_Star
	}
	s->d = 0;
	s->f = 0;	// Only necessary for A_Star
}

template <typename VertexType>
void Edge<VertexType>::relax(void)
{
	if (v->d > (u->d + weight))
	{
		v->d = u->d + weight;
		v->predecessor = u;
	}
}

template <typename Type>
bool AdjacencyGraph<Type>::Bellman_Ford(DFSVertex<Type>* s)
{
//	AdjacencyGraph<VertexType>& G = this;
	this->InitializeSingleSource(s);
	// for i = 1 to |G.V|-1
	for (unsigned int i = 1; i<=this->size()-1; ++i)
		// for each edge(u,v) € G.E
		for (typename AdjacencyGraph<Type>::iterator vertex=this->begin(); vertex!=this->end(); ++vertex)
			for (typename AdjacencyList<Type>::iterator edge=vertex->second->begin(); edge!=vertex->second->end(); ++edge)
				edge->relax();

	// for each edge(u,v) € G.E
	for (typename AdjacencyGraph<Type>::iterator vertex=this->begin(); vertex!=this->end(); ++vertex)
		for (typename AdjacencyList<Type>::iterator edge=vertex->second->begin(); edge!=vertex->second->end(); ++edge)
			if (edge->v->d > edge->u->d + edge->weight)
				return false;

	return true;
}


template <typename Type>
void AdjacencyGraph<Type>::DAG_Shortest_Path(DFSVertex<Type>* s)
{
//	AdjacencyGraph<VertexType>& G = this;
	typedef DFSVertex<Type> IntVertex;
	list<IntVertex*> topoList = this->TopologicalSort();
	this->InitializeSingleSource(s);
	for (typename list<IntVertex*>::iterator u = topoList.begin(); u!=topoList.end(); ++u)
		for (typename AdjacencyList<Type>::iterator edge=(*this)[*u]->begin(); edge!=(*this)[*u]->end(); ++edge)
		{
//			cout << "\tRelaxing " << *edge;
			edge->relax();
//			cout << " => " << *edge << endl;
		}
}

template <typename Type>
void AdjacencyGraph<Type>::Dijkstra(DFSVertex<Type>* s)
{
	// 0. Create Empty Set
//	set<IntVertex*> S;	// Really not used at all!!!
	// 1. Create min-priority Q
	// 	  it seems that std::priority_queue doesn't sort when 'd' is changed, only when element insertion
	list<DFSVertex<Type>*> Q;
	sortVertexByDistance<Type> comparison;
	// 2. InitializeSingleSource and Q
	for (typename AdjacencyGraph<Type>::iterator vertex=this->begin(); vertex!=this->end(); ++vertex)
	{
		vertex->first->d=INT_MAX;
		vertex->first->predecessor=NULL;
		Q.push_back(vertex->first);
	}
	s->d = 0;
	Q.sort(comparison);
	unsigned int iteration=0;	// For debuging
	while (!Q.empty())
	{
		DFSVertex<Type>* u=Q.front();
		Q.erase(Q.begin());
//		S.insert(u);
		for (typename AdjacencyList<Type>::iterator edge=(*this)[u]->begin(); edge!=(*this)[u]->end(); ++edge)
		{
//			cout << "\tRelaxing " << *edge;
			edge->relax();
//			cout << " => " << *edge << endl;
			++iteration;
		}
		Q.sort(comparison);
	}
	cout << "Dijkstra finished after " << iteration << " iterations" << endl;
}

template <typename Type>
void AdjacencyGraph<Type>::A_Star(DFSVertex<Type>* s, DFSVertex<Type>* destination)
// distance stores cost-so-far
// finishing time stores estimated-total-cost
{
	// 0. Create Empty Set
//	set<IntVertex*> S;	// Really not used at all!!!
	// 1. Create min-priority Q
	// 	  it seems that std::priority_queue doesn't sort when 'd' is changed, only when element insertion
	list<DFSVertex<Type>*> Q;
	sortVertexByFinishingTime<Type> comparison;
	// 2. InitializeSingleSource and Q
	for (typename AdjacencyGraph<Type>::iterator vertex=this->begin(); vertex!=this->end(); ++vertex)
	{
		vertex->first->d=INT_MAX;
		vertex->first->f=INT_MAX;
		vertex->first->predecessor=NULL;
		Q.push_back(vertex->first);
	}
	s->d = 0;
	s->f = 0;
	Q.sort(comparison);
	unsigned int iteration=0;	// For debuging
	while (!Q.empty() && Q.front()!=destination)
	{
		DFSVertex<Type>* u=Q.front();
		Q.erase(Q.begin());
//		S.insert(u);
		for (typename AdjacencyList<Type>::iterator edge=(*this)[u]->begin(); edge!=(*this)[u]->end(); ++edge)
		{
//			cout << "\tRelaxing " << *edge;
			// Relaxing Using Heuristics
			// Relaxes 'f' instead of d and using heuritic for 'VertexType'
			typename DFSVertex<Type>::timestamp_t heuristic = AStar_Heuristic(edge->v, destination);
//			cout << " heuristic="<<heuristic << endl;
			if (edge->v->f > (edge->u->d + edge->weight + heuristic))
			{
				edge->v->d = edge->u->d + edge->weight;
				edge->v->predecessor = edge->u;
				edge->v->f = edge->v->d + heuristic;

				++iteration; // For debuging
			}
//			cout << "\t\t => " << *edge << endl;
		}
		Q.sort(comparison);
	}

	cout << "A* finished after " << iteration << " iterations" << endl;
}

template <typename Type>
void Hypergraph<Type>::addVertex(Type& u)
{
	House_iterator uIterator = find(house.begin(), house.end(), u);	// Search for 'u' in the house
	// If 'u' is not in the house
	if (uIterator==house.end()) // 'u' is not in the house
	{
//		cout << "'u' is not in the House" << endl;
		// Add 'u' to the House
		house.push_back(u);
		Vertex_t vertex;
		vertex.value=&house.back();
		vertexHouse.push_back(vertex);
		// Create a new adjacency list for 'uVertex'
		List_t newList;
		listHouse.push_back(newList);
		List_t* uList = &listHouse.back();
//		cout << "Created new uList of size " << uList->size() << " listHouse.size()="<< listHouse.size() << endl;
		(*this)[&vertexHouse.back()] = uList;	// Added reference from Hypergraph to ListHouse
	}
}

template <typename Type>
void Hypergraph<Type>::addEdge(Type& u, Type& v, real w)
{
//	cout << "addEdge("<< u << "," << v << "," << w <<")"<< endl;
	// 1. Search for 'u' in the house and point at it
	Vertex_t* uVertex = 0;
	House_iterator uIterator = find(house.begin(), house.end(), u);	// Search for 'u' in the house
	// 1A. 'u' is not in the house
	if (uIterator==house.end()) // 'u' is not in the house
	{
//		cout << "'u' is not in the House" << endl;
		// Add 'u' to the House
		house.push_back(u);
		Vertex_t vertex;
		vertex.value=&house.back();
		vertexHouse.push_back(vertex);
		uVertex = &vertexHouse.back(); // vVertex points to the newly created vertex from 'v'
//		cout << "Created U=" << *uVertex << " vertexHouse.size()=" << vertexHouse.size() << endl;
	}
	else // 1B. 'u' is in the House
	{
//		cout << "'u' is in the House" << endl;
		// We need to find the vertex of 'u' in the VertexHouse
		VertexHouse_iterator uVertexIterator=vertexHouse.begin();
		while (uVertex==0 && uVertexIterator!=vertexHouse.end())
		{
			if (uVertexIterator->value == &(*uIterator))
				uVertex=&(*uVertexIterator);
			++uVertexIterator;
		}
	}
//	cout << "uVertex=" << *uVertex << "(Line "<< __LINE__ << ")" << endl;
	// 2. Now uVertex points to the vertex of 'u', next we need to find 'v' and point to it
	Vertex_t* vVertex = 0;
	House_iterator vIterator = find(house.begin(), house.end(), v);	// Search for 'v' in the house
	// 2A. 'v' is not in the house
	if (vIterator==house.end()) // 'v' is not in the house
	{
//		cout << "'v' is not in the House" << endl;
		// Add 'v' to the House
		house.push_back(v);
		Vertex_t vertex;
		vertex.value=&house.back();
		vertexHouse.push_back(vertex);
		vVertex = &vertexHouse.back(); // vVertex points to the newly created vertex from 'v'
//		cout << "Created V=" << *vVertex << " vertexHouse.size()=" << vertexHouse.size() << endl;
	}
	else // 2B. 'v' is in the House
	{
//		cout << "'v' is in the House" << endl;
		// We need to find the vertex of 'v' in the VertexHouse
		VertexHouse_iterator vVertexIterator=vertexHouse.begin();
		while (vVertex==0 && vVertexIterator!=vertexHouse.end())
		{
			if (vVertexIterator->value == &(*vIterator))
				vVertex=&(*vVertexIterator);
			++vVertexIterator;
		}
	}
//	cout << "vVertex=" << *vVertex << "(Line "<< __LINE__ << ")" << endl;
	// 3. Now uVertex and vVertex points to the verteces of 'u' and 'v', respectively
	//    Next we need to find edgeUV
	// 	  First, check if 'u' has any connection (adjacency list of vertex 'u')
	List_t* uList = (*this)[&(*uVertex)];
//	cout << "Adjacency List of 'u'" << (uList? " exists" : " does not exist") << endl;
	if (!uList) // Vertex 'u' doesn't have any adjacency list
	{
		// Create a new adjacency list for 'uVertex'
		List_t newList;
		listHouse.push_back(newList);
		uList = &listHouse.back();
//		cout << "Created new uList of size " << uList->size() << " listHouse.size()="<< listHouse.size() << endl;
		(*this)[&(*uVertex)] = uList;	// Added reference from Hypergraph to ListHouse
	}
	// 4. Now uList points to the adjacency list of the vertex 'u'
	// 	  So we can find vVertex in any of the edges of the adjacency list
	typename List_t::iterator edge=uList->begin();
	while (edge->v!=vVertex && edge!=uList->end())
	{
//		cout << "Checking edge " << *edge << endl;
		++edge;
	}
	//	Now edge points either to the edgeUV or to end of AdjacencyList of 'uVertex'
	if (edge==uList->end())
	{
//		cout << "EdgeUV not found" << endl;
//		cout << "I will create an edge from U=" << *uVertex << " to V=" << *vVertex << endl;
		// the edge UV is not there
		// we need to create a new edgeUV in the edgeHouse and add it to the uList
		Edge_t edgeUV;
		edgeUV.u=&(*uVertex);
		edgeUV.v=vVertex;
		edgeUV.weight=w;
		edgeHouse.push_back(edgeUV);
		uList->push_back(&edgeHouse.back());
//		cout << "Created new edgeUV " << edgeHouse.back() << " edgeHouse.size()="<< edgeHouse.size() << endl;
//		cout << "uList=" << *uList << endl;
		// now uVertex has a connections to vVertex through and edgeUV in the adjacency list 'uList'
	}
	// the edge UV is there and present in G[&u]
	// No need to add anything
	// DONE!
}

template <typename Type>
typename Hypergraph<Type>::Vertex_t* Hypergraph<Type>::getPointer(const Type& u) const
{
	Vertex_t* pointer=0;
	Hypergraph_const_iterator it=this->begin();
	while (!pointer && it!=this->end())
	{
		if (*(it->first->value)==u)
			pointer = it->first;
		++it;
	}
	return pointer;
}

template <typename Type>
typename Hypergraph<Type>::Vertex_t* Hypergraph<Type>::getPointer(const Type& u, real distance) const
// General Implementation (for example, number types
{
	real d = distance*distance; // absolute value
	Vertex_t* pointer=0;
	Hypergraph_const_iterator it=this->begin();
	while (!pointer && it!=this->end())
	{
		real diff = *(it->first->value) - u;
		diff *= diff;	// absolute value
		if (diff <= d)
			pointer = it->first;
		++it;
	}
	return pointer;
}

} // end namespace neurignacio
#endif /* GRAPH_H_ */
